#pragma once

#include <gdnative/color.h>

#include <math.h>
#include <algorithm>

namespace Math {

	template <class T>
	inline const T& clamp(const T& p_x, const T& p_min, const T& p_max) {
		return std::max(p_min, std::min(p_max, p_x));
	}

	inline float ease(float p_x, float p_c) {
		p_x = clamp(p_x, 0.0f, 1.0f);
		if (p_c > 0.0f) {
			if (p_c < 1.0f) return 1.0f - powf(1.0f - p_x, 1.0f / p_c);
			else return powf(p_x, p_c);
		} else if (p_c < 0.0f) {
			//inout ease
			if (p_x < 0.5f) return powf(p_x * 2.0f, -p_c) * 0.5f;
			else return (1.0f - powf(1.0f - (p_x - 0.5f) * 2.0f, -p_c)) * 0.5f + 0.5f;
		} else {
			return 0.0f; // no ease (raw)
		}
	}

	inline float deg2rad(float deg) {
		return deg * (3.14159265f / 180.0f);
	}

	inline float rad2deg(float rad) {
		return rad * (180.0f / 3.14159265f);
	}

}

namespace Color {

	inline uint32_t to_rgbe9995(float r, float g, float b) {

		const float pow2to9 = 512.0f;
		const float B = 15.0f;
		//const float Emax = 31.0f;
		const float N = 9.0f;

		const float sharedexp = 65408.000f; //(( pow2to9  - 1.0f)/ pow2to9)*powf( 2.0f, 31.0f - 15.0f);

		float cRed = Math::clamp(r, 0.0f, sharedexp);
		float cGreen = Math::clamp(g, 0.0f, sharedexp);
		float cBlue = Math::clamp(b, 0.0f, sharedexp);

		float cMax = std::max(cRed, std::max(cGreen, cBlue));

		// expp = MAX(-B - 1, log2(maxc)) + 1 + B

		float expp = std::max(-B - 1.0f, floorf(logf(cMax) / 0.693147181f)) + 1.0f + B;

		float sMax = floorf((cMax / powf(2.0f, expp - B - N)) + 0.5f);

		float exps = expp + 1.0f;

		if (0.0f <= sMax && sMax < pow2to9) {
			exps = expp;
		}

		uint32_t sRed = uint32_t((cRed / powf(2.0f, exps - B - N)) + 0.5f);
		uint32_t sGreen = uint32_t((cGreen / powf(2.0f, exps - B - N)) + 0.5f);
		uint32_t sBlue = uint32_t((cBlue / powf(2.0f, exps - B - N)) + 0.5f);

		return (sRed & 0x1FF) | ((sGreen & 0x1FF) << 9) | ((sBlue & 0x1FF) << 18) | ((uint32_t(exps) & 0x1F) << 27);
	}

	inline uint32_t to_rgbe9995(const godot_color& p_src) {
		return to_rgbe9995(p_src.r, p_src.g, p_src.b);
	}

	inline godot_color blend(const godot_color& p_src, const godot_color& p_over) {
		float sa = 1.0f - p_over.a;
		float res_a = p_src.a * sa + p_over.a;
		if (res_a == 0.0f) {
			return godot_color{ 0.0f, 0.0f, 0.0f, 0.0f };
		} else {
			return godot_color{
				(p_src.r * p_src.a * sa + p_over.r * p_over.a) / res_a,
				(p_src.g * p_src.a * sa + p_over.g * p_over.a) / res_a,
				(p_src.b * p_src.a * sa + p_over.b * p_over.a) / res_a,
				res_a
			};
		}
	}

	inline godot_color linear_interpolate(const godot_color& c1, const godot_color& c2, float factor) {
		return godot_color{
			c1.r + (c2.r - c1.r) * factor,
			c1.g + (c2.g - c1.g) * factor,
			c1.b + (c2.b - c1.b) * factor,
			c1.a + (c2.a - c1.a) * factor
		};
	}

	extern const float srgb_to_linear_lut[256];

	inline float to_linear(float r) {
		return r < 0.04045f ? r * (1.0f / 12.92f) : powf((r + 0.055f) * (1.0f / 1.055f), 2.4f);
	}

	inline godot_color to_linear(const godot_color& p_src) {
		return godot_color{ to_linear(p_src.r), to_linear(p_src.g), to_linear(p_src.b), p_src.a };
	}

	inline float to_linear(uint8_t r) {
		return srgb_to_linear_lut[r];
	}

	inline godot_color to_linear(uint32_t clr) {
		return godot_color{
			to_linear((uint8_t)clr),
			to_linear((uint8_t)(clr >> 8)),
			to_linear((uint8_t)(clr >> 16)),
			(float)(uint8_t)(clr >> 24) / 255.0f
		};
	}

	inline float to_srgb(float r) {
		return r < 0.0031308f ? 12.92f * r : 1.055f * powf(r, 1.0f / 2.4f) - 0.055f;
	}

	inline godot_color to_srgb(const godot_color& p_src) {
		return godot_color{ to_srgb(p_src.r), to_srgb(p_src.g), to_srgb(p_src.b), p_src.a };
	}

}
